; Dit programma krijgt via de seriele poort 4 bytes binnen: 1 met info over de Ledjes (merk op
; dat slecht 7 ledjes gebruikt worden om niet in conflict te komen met de synchronisatie), de 
; 2 stelwaarden voor de hoek van 2 servomotoren en een 0. Deze nul dient om te synchroniseren. 
; De binnengekomen waarde wordt gestockeerd in DataIn1 en telkens 1 opgeschoven. Als de nul 
; gedetecteerd wordt, worden de 2 waarden gecopieerd in reqpos1 en 2 en de data voor de ledjes
; op poort B gestuurd. In het hoofdprogramma worden deze waarden gecopieerd in timpos1 en 2. 
; De uitgangen worden hoog gezet en timpos1 en 2 worden telkens verlaagd met 1. als ze nul 
; worden wordt de respectievelijke uitgang ook op nul gezet. Hierdoor wordt een soort PWM 
; signaal bekomen dat gebruikt kan worden om een servo motor te sturen. Ook wordt een
; analoge ingang uitgelezen en doorgestuurd naar de PC. Merk op dat de PWM in het hoofdprog
; wordt gemaakt en dus veel processor tijd in beslag neemt!


	LIST   P=PIC16F876a
	#INCLUDE P16F876a.INC
	;errorlevel -302		;suppress "not in bank 0" message

	__CONFIG	_CP_OFF & _DEBUG_OFF & _LVP_OFF & _PWRTE_ON & _WDT_OFF & _BODEN_OFF & _XT_OSC


	CBLOCK	0x20
	WREG_TEMP		;storage for WREG during interrupt
	STATUS_TEMP		;storage for STATUS during interrupt
	PCLATH_TEMP		;storage for PCLATH during interrupt
	FSR_TEMP		;storage for FSR during interrupt
	TIMER1          	;Timer 1, Used for general delay.
	TIMER2          	;Timer 2, Used for general delay.		
	OUTPUT	
	DATAIN1			;Recieved data	
	DATAIN2			;Recieved data	
	DATAIN3			;Recieved data	
	DATAIN4			;Recieved data	
	ENDC

	CBLOCK	0x30
	TimMax			; Max Timer
	ReqPos1			; Required position servo 1
	TimPos1			; CountdownTimer position servo 1
	ReqPos2			; Required position servo 2
	TimPos2			; CountdownTimer position servo 2
	ENDC

;----------------------------------------------------------------------------
;RESET Interrupt Vector
		ORG     0x0000		;reset vector
 		GOTO	Main
		nop
		nop		
;----------------------------------------------------------------------------
;Other interrupt vector
		ORG	0x0004		;interrupt vector
		GOTO 	Int

;******************************************************************************
;******************************************************************************

Main:		
		Bank1
		BCF	TRISC,1
		BCF	TRISC,2
		MOVLW	B'10000000'
		MOVWF	TRISB
		Bank0
		CALL	SetupSerial
		CALL	SetupAnalog
		Bank0
		MOVLW	D'0'    
		MOVWF	ReqPos1
		MOVLW	D'0'
		MOVWF	ReqPos2

MAINLOOP
		MOVF	ReqPos1,W	
		MOVWF	TimPos1		; copy reqpos to timpos
		MOVF	ReqPos2,W
		MOVWF	TimPos2
		MOVLW	D'255'
		MOVF	TimMax

		MOVLW	B'00000110'
		MOVWF	OUTPUT
		MOVWF	PORTC
LOOP
		DECF	TimMax,F
		DECFSZ	TimPos1,F	; count down until timpos = 0
		GOTO	If1
		BCF	OUTPUT,1	; then clear output
If1
		DECFSZ	TimPos2,F	; count down until timpos = 0
		GOTO	If2
		BCF	OUTPUT,2	; then clear output
If2
		MOVF	OUTPUT,W
		MOVWF	PORTC
		
		INCF	TimMax,F	
		DECFSZ	TimMax,F	; All Outputs done ?
		GOTO	LOOP

		BTFSS	PIR1,TXIF	;check if transmitter busy	
		GOTO	If3
		
		BSF	ADCON0,GO	;Start AD conv
		BTFSC	ADCON0,GO	;Wait until AD conv is done
		MOVF	ADRESH,W
		MOVWF	TXREG		;and transmit
If3
		CALL	ShortDelay
		GOTO 	MAINLOOP

;******************************************************************************
;******************************************************************************
SetupSerial:	Bank1			;select bank 1
		BCF	TRISC,6		; Make RC6 - TX pin an output
		BSF	TRISC,7		; Make RC7 - RX pin an input
		BSF	TXSTA,BRGH	;Select low speed mode	
		MOVLW	D'25'
		MOVWF	SPBRG		;Select baud rate 9600 in low speed	
		BCF	TXSTA,SYNC	;Select async mode	
		BSF	TXSTA,TXEN	;Enable Transmit

		Bank0			;select bank0
		movlw	0x90		;enable serial port and reception
		movwf	RCSTA
		movlw	0xc0		;enable global and peripheral ints
		movwf	INTCON
		Bank1			;select bank1
		movlw	0x20		; (0x30 enable TX) and RX interrupts
		movwf	PIE1
		Bank0
		return

SetupAnalog:
		Bank1
		MOVLW	B'01000000'
		MOVWF	ADCON1
		Bank0
		MOVLW	B'01000001'
		MOVWF	ADCON0
		RETURN

ShortDelay:		
		MOVLW   D'30'          
                MOVWF   TIMER1         
DELAY2		MOVLW	D'255'
		MOVWF	TIMER2
		DECFSZ  TIMER2,F        
                GOTO    $-1             
		DECFSZ	TIMER1,F
		GOTO	DELAY2	
		RETLW   0

;******************************************************************************
;******************************************************************************
Int:		movwf	WREG_TEMP	;save WREG
		movf	STATUS,W	;store STATUS in WREG
		clrf	STATUS		;select file register bank0
		movwf	STATUS_TEMP	;save STATUS value
		movf	PCLATH,W	;store PCLATH in WREG
		movwf	PCLATH_TEMP	;save PCLATH value
		clrf	PCLATH		;select program memory page0
		movf	FSR,W		;store FSR in WREG
		movwf	FSR_TEMP	;save FSR value

		Bank0			;select bank0
		btfsc	PIR1,RCIF	;test RCIF receive interrupt
		bsf	STATUS,RP0	;change to bank1 if RCIF set
		btfsc	PIE1,RCIE	;test if interrupt enabled if RCIF set
		goto	GetData		;if RCIF and RCIE set, do receive

		Bank0			;select bank0
		btfsc	PIR1,TXIF	;test for TXIF transmit interrupt
		bsf	STATUS,RP0	;change to bank1 if TXIF set
		btfsc	PIE1,TXIE	;test if interrupt enabled if TXIF set
		goto	PutData		;if TXIF and TCIE set, do transmit

;can do special error handling here - an unexpected interrupt occurred 
EndInt:		Bank0			;select bank 0
		movf	FSR_TEMP,W	;get saved FSR value
		movwf	FSR		;restore FSR
		movf	PCLATH_TEMP,W	;get saved PCLATH value
		movwf	PCLATH		;restore PCLATH
		movf	STATUS_TEMP,W	;get saved STATUS value
		movwf	STATUS		;restore STATUS
		swapf	WREG_TEMP,F	;prepare WREG to be restored
		swapf	WREG_TEMP,W	;restore WREG without affecting STATUS
		retfie			;return from interrupt

;******************************************************************************
;******************************************************************************
;Get received data 
GetData:	Bank0
		btfsc	RCSTA,OERR	;test overrun error flag
		goto	ErrOERR		;handle it if error
		btfsc	RCSTA,FERR	;test framing error flag
		goto	ErrFERR		;handle it if error

		movf	DATAIN3,W	;Move last received data		
		movwf	DATAIN4

		movf	DATAIN2,W	;Move last received data		
		movwf	DATAIN3

		movf	DATAIN1,W	;Move last received data		
		movwf	DATAIN2

		movf	RCREG,W		;Get received data
		movwf	DATAIN1		; Copy to DataIn1

		SUBLW	D'0'
		BTFSS	STATUS,Z	; test if last data was 0
		GOTO	BlockNotComplete

		Bank0
		movf	DATAIN3,W	;Move data to ReqPos1		
		movwf	ReqPos1	

		movf	DATAIN2,W	;Move data to ReqPos1		
		movwf	ReqPos2

		movf	DATAIN4,W	;Move data 	
		ANDWF	B'01111111'
		MOVWF	PORTB
		
BlockNotComplete:
		goto	EndInt

;error because OERR overrun error bit is set - clear and continue
ErrOERR:	bcf	RCSTA,CREN	;reset the receiver logic
		bsf	RCSTA,CREN	;enable reception again
		goto	EndInt
;error because FERR framing error bit is set - clear and continue
ErrFERR:	movf	RCREG,W		;discard received data that has error
		goto	EndInt

;******************************************************************************
;Read analog data and transmit the data.
PutData:	
		GOTO	EndInt



		END